#include <stdio.h>
#include <string>
#include "AstarNode.h"
#include "Astar.h"
#include <pluginlib/class_list_macros.h>
#include <ctime>
PLUGINLIB_EXPORT_CLASS(Astar_planner::AstarPlannerRos,nav_core::BaseGlobalPlanner)

int mapSize;
float originX;
float originY;
int width;
int hight;
float resolution;

namespace Astar_planner
{
    AstarPlannerRos::AstarPlannerRos()
    {

    }

    AstarPlannerRos::AstarPlannerRos(ros::NodeHandle &nh)
    {
        ROSNodeHandle=nh;
    }

    AstarPlannerRos::AstarPlannerRos(std::string name,costmap_2d::Costmap2DROS* costmap_ros)
    {
        initialize(name,costmap_ros);
    }
    void AstarPlannerRos::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
    {
       nh.param("distance",m_distance,std::string("euclidean"));
       nh.param("weight_a",m_weight_a,1.0);
       nh.param("weight_b",m_weight_b,1.0);
    }
    void AstarPlannerRos::initialize(std::string name,costmap_2d::Costmap2DROS* costmap_ros)
    {
       cout<<"######### hello astar plugins ##########"<<endl;
        if (!initialize_)
        {
            costmap_ros_=costmap_ros;
            costmap_=costmap_ros_->getCostmap();
            originX = costmap_->getOriginX();
            originY = costmap_->getOriginY();
            width = costmap_->getSizeInCellsX();
            hight = costmap_->getSizeInCellsY();
            resolution = costmap_->getResolution();

            ros::NodeHandle nh("~/"+name);
            loadRosParamFromNodeHandle(nh);

            mapSize = width*hight;
            cout<<"************msg message**********"<<endl;
            cout<<"originX:"<<originX<<endl;
            cout<<"originY:"<<originY<<endl;
            cout<<"width:"<<width<<endl;
            cout<<"height:"<<hight<<endl;
            cout<<"resolution:"<<resolution<<endl;
            cout<<"*********************************"<<endl;
            m_mapData.resize(mapSize);
            m_width=width;
            m_hight=hight;
            for (int iy=0;iy<costmap_->getSizeInCellsY();iy++)
            {
                for (int ix=0;ix<costmap_->getSizeInCellsX();ix++)
                {
                   unsigned int cost=static_cast<int>(costmap_->getCost(ix,iy));
                    if (cost==0)
                        m_mapData[iy*width+ix]=0;
                    else
                        m_mapData[iy*width+ix]=100;
                }
            }
            ROS_INFO("Astar planner initialized successfully");
            initialize_ = true;
        }
        else
           ROS_WARN("This planner has already been initialized... doing nothing");
    }

    bool AstarPlannerRos::makePlan(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan)
    {
        cout<<"######hello astar plan######"<<endl;
        if (!initialize_)
        {
            ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
            return false;
        }
		
        ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);
        plan.clear();

        if (goal.header.frame_id != costmap_ros_->getGlobalFrameID())
        {
            ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
            return false;
        }
        tf::Stamped < tf::Pose > goal_tf;
        tf::Stamped < tf::Pose > start_tf;
        poseStampedMsgToTF(goal, goal_tf);
        poseStampedMsgToTF(start, start_tf);
        float startX = start.pose.position.x;
        float startY = start.pose.position.y;
        float goalX = goal.pose.position.x;
        float goalY = goal.pose.position.y;
        
        cout<<"startX:"<<startX<<" "<<"startY:"<<startY<<endl;
        cout<<"goalX:"<<goalX<<" "<<"goalY:"<<goalY<<endl;

		//定义起点
		std::vector<std::vector<int> >start_map_point;
        start_map_point.clear();
		start_map_point.push_back(WorldTomap(startX,startY));
        cout<<"start:"<<"("<<start_map_point[0][0]<<","<<start_map_point[0][1]<<")"<<endl;
		if (start_map_point[0][0]==-1&&start_map_point[0][1]==-1)
            std::cout<<"\033[0;31m[E] : Please set the valid goal point\033[0m"<<endl;
		
		//定义终点
		std::vector<std::vector<int> >goal_map_point;
        goal_map_point.clear();
		goal_map_point.push_back(WorldTomap(goalX,goalY));
        cout<<"goal:"<<"("<<goal_map_point[0][0]<<","<<goal_map_point[0][1]<<")"<<endl;
		if (goal_map_point[0][0]==-1&&goal_map_point[0][1]==-1)
			std::cout<<"\033[0;30m[Kamerider E] : Please set the valid goal point\033[0m"<<endl;
		
		//开始Astar寻路
        int xStart=start_map_point[0][1];
        int yStart=start_map_point[0][0];

        int xStop=goal_map_point[0][1];
        int yStop=goal_map_point[0][0];
        // float weight_a=1.0;
        // float weight_b=1.0;
        // std::string distance="euclidean";

        ASTAR::CAstar astar(xStart, yStart, xStop, yStop, m_weight_a, m_weight_b,ASTAR::CAstar::PathType::NOFINDPATHPOINT,m_distance);
        astar.InitMap(m_mapData,m_width,m_hight);

        std::vector<std::pair<int, int> >astarPath;   //Astar算法的路径点
        clock_t time_stt=clock();
        astarPath=astar.PathPoint();
        cout<<"\033[0;32m[W] :Astar Get Time:\033[0m"<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
        cout<<"astar size:"<<astarPath.size()<<endl;

        if (astarPath.size()>0)
        {
            for (int i=astarPath.size()-1;i>-1;i--)
            {
               geometry_msgs::PoseStamped current_pose=goal;
                float x=0.0,y=0.0;
                x=MapToWorld(astarPath[i].first,astarPath[i].second)[0];
                y=MapToWorld(astarPath[i].first,astarPath[i].second)[1];
                current_pose.pose.position.x=x;
                current_pose.pose.position.y=y;
                current_pose.pose.position.z = 0.0;

                current_pose.pose.orientation.x = 0.0;
                current_pose.pose.orientation.y = 0.0;
                current_pose.pose.orientation.z = 0.0;
                current_pose.pose.orientation.w = 1.0;
                
                plan.push_back(current_pose);
            }
               return  true;
        }
        else
        {
            ROS_WARN("Not valid start or goal");
            return false;
        }
		
    }
	
	std::vector<int> AstarPlannerRos::WorldTomap(double wx,double wy)
	{
		std::vector<int> v;
        v.clear();
		if (wx<(1.0*originX) || wy<(1.0*originY))
		{
			v.push_back(-1);
			v.push_back(-1);
			return v;
	    }
	    int mx=int((1.0*(wx-originX))/resolution);
	    int my=int((1.0*(wy-originY))/resolution);
		if (mx<width && my<hight)
		{
			v.push_back(mx);
			v.push_back(my);
			return v;
		}

    }

    std::vector<double> AstarPlannerRos::MapToWorld(int my,int mx)
    {
	   std::vector<double> v;
       v.clear();
	   if(mx>width || my>hight)
	   {
		   v.push_back(-1);
		   v.push_back(-1);
		   return v;
	   }
	   double wx=(mx*resolution+originX);
	   double wy=(my*resolution+originY);
	   if (wx>originX&&wy>originY)
	   {
		   v.push_back(wx);
		   v.push_back(wy);
		   return v;
	   }

    }

};
